


def get_state(envs):
    object = envs.envs[0].unwrapped.sim._bodies_idx['object']
    table = envs.envs[0].unwrapped.sim._bodies_idx['table']
    robot = envs.envs[0].unwrapped.sim._bodies_idx['panda']
    contact_points = envs.envs[0].unwrapped.sim.physics_client.getContactPoints(bodyA=table, bodyB=object, linkIndexA=-1,linkIndexB = -1)
    contact_points1 = envs.envs[0].unwrapped.sim.physics_client.getContactPoints(bodyA = robot,bodyB = object, linkIndexA = 9, linkIndexB = -1)
    contact_points2 = envs.envs[0].unwrapped.sim.physics_client.getContactPoints(bodyA = robot,bodyB = object, linkIndexA = 10, linkIndexB = -1)
    object_info = envs.envs[0].unwrapped.sim.physics_client.getBasePositionAndOrientation(bodyUniqueId=object)
    # fingers_width = envs.envs[0].unwrapped.robot.get_fingers_width()

    at_high = object_info[0][2] > 0.021
    # clamp_finger = fingers_width > 0.03 and fingers_width < 0.0405
    zero_table_contact = len(contact_points) == 0
    contact_with_two_fingers = (len(contact_points1) > 0 and len(contact_points2)) > 0
    
    if at_high and zero_table_contact and contact_with_two_fingers:
        return 'pickandplace'
    elif at_high:
        return 'roll'
    elif object_info[0][2] > 0.019 and object_info[0][2]<0.21:
        return 'push'
    else:
        return 'down'

def states_to_result(states):
    if 'pickandplace' in states:
        return 'pickandplace'
    elif 'roll' in states:
        return 'roll'
    else:
        return 'push'
